using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Core.DsspHttp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Robotics.PhysicalModel.Proxy;
using System;
using System.Net;
using System.Collections.Generic;
using System.ComponentModel;
using System.Security.Permissions;

using Microsoft.Dss.Core.DsspHttpUtilities;
using System.Collections.Specialized;

using W3C.Soap;
using motor = Microsoft.Robotics.Services.Motor.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using vex = VEXRobotics.Proxy;
using r = Microsoft.Robotics;

/* Notes on VEX Motor
 *   Identifier must be the motor port number on the VEX. Range: 1 - 8.
 */
namespace VEXService.Motor
{
    [ServicePort]
    public class VEXMotorOperations : PortSet<
        DsspDefaultLookup,
        DsspDefaultDrop,
        Get,
        motor.Replace,
        motor.SetMotorPower,
        HttpGet,
        HttpPost>
    { }

    /// <summary>
    /// Get the state
    /// </summary>
    [DataContract]
    public class Get : Get<GetRequestType, PortSet<VEXMotorState, Fault>>
    {
    }

    /// <summary>
    /// VEX Motor State
    /// </summary>
    [DataContract]
    [Description("Specifies the VEX Motor state.")]
    public class VEXMotorState : IDssSerializable, ICloneable
    {
        private string _Name;
        private vex.MotorPort _MotorPort;
        private bool _ReversePolarity;
        private Pose _Pose;
        private double _CurrentPower;
        private double _PowerScalingFactor;
        private DateTime _TimeStamp;

        public VEXMotorState()
        {
            _PowerScalingFactor = 1.0;
            _Pose = new Pose();
        }

        /// <summary>
        /// VEX Motor Port.
        /// </summary>
        [DataMember, Description("Specifies the VEX Motor Port.")]
        public vex.MotorPort MotorPort
        {
            get { return _MotorPort; }
            set { _MotorPort = value; }
        }

        /// <summary>
        /// Specifies the descriptive identifier for the motor.
        /// </summary>
        [DataMember, Description("Specifies a user friendly name for the VEX Motor.")]
        public string Name
        {
            get { return _Name; }
            set { _Name = value; }
        }

        /// <summary>
        /// Indicates the direction (polarity) of the motor.
        /// (Enabling this option (true) reverses the motor)
        /// </summary>
        [DataMember, Description(@"Indicates the direction (polarity) of the motor.\n(Enabling this option (true) reverses the motor.)")]
        public bool ReversePolarity
        {
            get { return _ReversePolarity; }
            set { _ReversePolarity = value; }
        }

        /// <summary>
        /// Pose of the motor
        /// </summary>
        [DataMember]
        [Description("The position and orientation of the motor.")]
        public Pose Pose
        {
            get { return _Pose; }
            set { _Pose = value; }
        }

        /// <summary>
        /// Indicates the current power applied to the motor; range is -1.0 to 1.0.
        /// </summary>
        [DataMember, Description("Indicates the current power applied to the motor; range is -1.0 to 1.0.")]
        [Browsable(false)]
        [DataMemberConstructor(Order = -1)]
        public double CurrentPower
        {
            get { return _CurrentPower; }
            set { _CurrentPower = value; }
        }

        //
        // Summary:
        //     Timestamp of this sample
        [DefaultValue(typeof(DateTime), "0001-01-01T00:00:00")]
        [Description("Identifies the timestamp of the motor reading.")]
        [Browsable(false)]
        [DataMember(Order = -1, XmlOmitDefaultValue = true)]
        public DateTime TimeStamp
        {
            get { return _TimeStamp; }
            set { _TimeStamp = value; }
        }


        /// <summary>
        /// Transform current state to the specified generic state.
        /// </summary>
        /// <param name="genericState"></param>
        /// <returns></returns>
        public motor.MotorState ToGenericState(motor.MotorState genericState)
        {
            genericState.Name = this.Name;
            genericState.HardwareIdentifier = (int)this.MotorPort;
            genericState.CurrentPower = this.CurrentPower;
            genericState.ReversePolarity = this.ReversePolarity;
            genericState.PowerScalingFactor = 1.0;
            return genericState;
        }

        /// <summary>
        /// Copy generic state to this state, transforming data members.
        /// </summary>
        /// <param name="genericState"></param>
        public void FromGenericState(motor.MotorState genericState)
        {
            this.CurrentPower = genericState.CurrentPower;
            this.ReversePolarity = genericState.ReversePolarity;
            this.Pose = genericState.Pose;
        }


        #region IDssSerializable

        /// <summary>
        /// Copy To Analog Input sensor state
        /// </summary>
        public virtual void CopyTo(IDssSerializable target)
        {
            VEXMotorState typedTarget = target as VEXMotorState;

            if (typedTarget == null)
                throw new ArgumentException("CopyTo({0}) requires type {0}", this.GetType().FullName);

            typedTarget.CurrentPower = this.CurrentPower;
            typedTarget.MotorPort = this.MotorPort;
            typedTarget.Name = this.Name;
            typedTarget.Pose = this.Pose;
            typedTarget.ReversePolarity = this.ReversePolarity;
            typedTarget.TimeStamp = this.TimeStamp;
        }

        /// <summary>
        /// Clone the VEX Motor State
        /// </summary>
        /// <returns></returns>
        public object Clone()
        {
            VEXMotorState clone = new VEXMotorState();
            clone.CurrentPower = this.CurrentPower;
            clone.MotorPort = this.MotorPort;
            clone.Name = this.Name;
            clone.Pose = this.Pose;
            clone.ReversePolarity = this.ReversePolarity;
            clone.TimeStamp = this.TimeStamp;

            return clone;
        }

        /// <summary>
        /// Serialize
        /// </summary>
        public virtual void Serialize(System.IO.BinaryWriter writer)
        {
            writer.Write(this.CurrentPower);
            writer.Write((int)this.MotorPort);
            writer.Write(Name);
            Pose.Serialize(writer);
            writer.Write(this.ReversePolarity);
            Microsoft.Dss.Services.Serializer.BinarySerializationHelper.SerializeDateTime(TimeStamp, writer);
        }

        /// <summary>
        /// Deserialize
        /// </summary>
        public virtual object Deserialize(System.IO.BinaryReader reader)
        {
            this.CurrentPower = reader.ReadDouble();
            this.MotorPort = (vex.MotorPort)reader.ReadInt32();
            this.Name = reader.ReadString();
            Pose.Deserialize(reader);
            this.ReversePolarity = reader.ReadBoolean();
            this.TimeStamp = Microsoft.Dss.Services.Serializer.BinarySerializationHelper.DeserializeDateTime(reader);

            return this;
        }
        #endregion
    }

    [Contract(VEXService.Motor.Contract.Identifier)]
    [AlternateContract(motor.Contract.Identifier)]
    [DisplayName("VEX Motor")]
    [Description("Provides access to the VEX motor.")]
    [DssServiceDescription("http://msdn.microsoft.com/en-us/library/dd145255.aspx")]
    [DssCategory(vex.VEXCategories.VEX)]
    public class VEXMotorService : DsspServiceBase
    {
        /// <summary>
        /// XSLT Transform to use for display
        /// </summary>
        [EmbeddedResource("VEXService.Config.Motor.xslt")]
        private string _transform = null;

        /// <summary>
        /// Motor state
        /// </summary>
        [InitialStatePartner(Optional = true, ServiceUri = ServicePaths.Store + @"/VEX.Motor.Config.xml")]
        VEXMotorState _State;

        /// <summary>
        /// Service port
        /// </summary>
        [ServicePort("VEX/Motor", AllowMultipleInstances = true)]
        private VEXMotorOperations _mainPort = new VEXMotorOperations();

        /// <summary>
        /// Alternate port for the generic port
        /// </summary>
        [AlternateServicePort("/motor",
            AllowMultipleInstances = true,
            AlternateContract = motor.Contract.Identifier)]
        private motor.MotorOperations _genericPort = new motor.MotorOperations();

        /// <summary>
        /// Port to talk with the VEX controller
        /// </summary>
        [Partner("VEX", Contract = vex.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate,
            Optional = false)]
        private vex.VEXOperations _vexPort = new vex.VEXOperations();
        private vex.VEXOperations _vexNotificationPort = new vex.VEXOperations();

        /// <summary>
        /// HTTP helper
        /// </summary>
        DsspHttpUtilitiesPort _httpUtilities = new DsspHttpUtilitiesPort();

        /// <summary>
        /// Subscription manager
        /// </summary>
        [Partner("SubMgr", Contract = submgr.Contract.Identifier,
            CreationPolicy = PartnerCreationPolicy.CreateAlways,
            Optional = false)]
        private submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();

        /// <summary>
        /// Constructor
        /// </summary>
        /// <param name="creationPort"></param>
        public VEXMotorService(DsspServiceCreationPort creationPort)
            :
                base(creationPort)
        {
        }

        /// <summary>
        /// Startup
        /// </summary>
        protected override void Start()
        {
            LogVerbose(LogGroups.Console, "Configuring VEX Motor... ");

            #region Initialize Service State
            if (_State == null)
            {
                _State = new VEXMotorState();
                SaveState(_State);
            }
            #endregion

			base.Start();

            #region Initialize Service State
            // Reset the controller to know about our configuration
            Port<bool> donePort = new Port<bool>();
            SpawnIterator<Port<bool>>(donePort, InitializeKnownConnection);
            Activate(Arbiter.Receive(false, donePort,
                delegate(bool success)
                {
                    if (success)
                    {
                        // display HTTP service Uri
                        LogInfo(LogGroups.Console, "Service uri: ");
                    }
                    else
                    {
                        LogError(LogGroups.Console, "VEX Motor service failed to start.");
                        _mainPort.Post(new DsspDefaultDrop());
                    }
                }));
            #endregion

            // Needed for HttpPost
            _httpUtilities = DsspHttpUtilitiesService.Create(Environment);
        }

        /// <summary>
        /// Find a valid motor to hook up to
        /// </summary>
        /// <returns></returns>
        private IEnumerator<ITask> InitializeKnownConnection(Port<bool> resultPort)
        {
            bool serviceSuccess = true;

            if (_State.MotorPort == vex.MotorPort.MotorPort_None)
            {
                LogError(_State.Name + ": No motor port selected");
                serviceSuccess = false;
            }
            else
            {
                vex.SelectiveMotorConfigure motorConfig = new vex.SelectiveMotorConfigure();
                motorConfig.Body.Motor = _State.MotorPort;
                motorConfig.Body.Type = "Continuous";
                _State.TimeStamp = DateTime.Now;

                _vexPort.Post(motorConfig);
                yield return Arbiter.Choice(motorConfig.ResponsePort,
                    delegate(DefaultReplaceResponseType response)
                    {
                    },
                    delegate(Fault fault)
                    {
                        serviceSuccess = false;
                        LogError(LogGroups.Console, "Failed to update VEX service configuration", fault);
                    });

                // Subscribe to VEX for notifications
                if (serviceSuccess)
                {
                    yield return Arbiter.Choice(
                        _vexPort.Subscribe(_vexNotificationPort, typeof(vex.Update)),
                        EmptyHandler,
                        delegate(Fault f)
                        {
                            serviceSuccess = false;
                        });
                }

                if (serviceSuccess)
                {
                    base.MainPortInterleave.CombineWith(new Interleave(
                        new ExclusiveReceiverGroup(
                            Arbiter.Receive<vex.Update>(true, _vexNotificationPort, SensorNotificationHandler)
                        ),
                        new ConcurrentReceiverGroup()));
                }
            }

            resultPort.Post(serviceSuccess);
            yield break;
        }

        /// <summary>
        /// Handle sensor update message from VEX
        /// </summary>
        private void SensorNotificationHandler(vex.Update notify)
        {
            if (_State.MotorPort != vex.MotorPort.MotorPort_None)
            {
                // Update state
                int newValue;
                newValue = notify.Body.Motors.Motors[(int)_State.MotorPort - 1].Value;
                int curPower = (int)(_State.CurrentPower * 127.0 + 127.0);
                bool changed = (curPower != newValue);
                _State.TimeStamp = DateTime.Now;
                _State.CurrentPower = (newValue - 127.0) / 127.0;

                // Notify subscribers on any state change
                if (changed)
                {
                    motor.MotorState genericState = new motor.MotorState();
                    SendNotification<motor.Replace>(_subMgrPort, _State.ToGenericState(genericState));
                }
            }
        }

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> GetHandler(Get get)
        {
            get.ResponsePort.Post(_State);
            yield break;
        }

        /// <summary>
        /// Get the state
        /// </summary>
        /// <param name="get"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> MotorGetHandler(motor.Get get)
        {
            motor.MotorState genericState = new motor.MotorState();
            get.ResponsePort.Post(_State.ToGenericState(genericState));
            yield break;
        }

        /// <summary>
        /// Get the state to HTTP display
        /// </summary>
        /// <param name="httpGet">Response port to post the state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        [ServiceHandler(ServiceHandlerBehavior.Concurrent, PortFieldName = "_genericPort")]
        public virtual IEnumerator<ITask> HttpGetHandler(HttpGet httpGet)
        {
            httpGet.ResponsePort.Post(new HttpResponseType(
                HttpStatusCode.OK,
                _State,
                _transform)
            );
            yield break;
        }

        /// <summary>
        /// HttpPost Handler
        /// </summary>
        /// <param name="submit"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public virtual IEnumerator<ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost);
            _httpUtilities.Post(readForm);

            // Wait for result
            Activate(Arbiter.Choice(readForm.ResultPort,
                delegate(NameValueCollection parameters)
                {
                    // Update our state variables
                    if (parameters["Name"] != null)
                    {
                        _State.Name = parameters["Name"];
                    }

                    bool changed = false;

                    bool reverse = false;
                    if (parameters["ReversePolarity"] != null)
                    {
                        reverse = true;
                    }
                    if(reverse != _State.ReversePolarity)
                    {
                        changed = true;
                    }
                    _State.ReversePolarity = reverse;

                    double power = _State.CurrentPower;
                    if (parameters["Power"] != null)
                    {
                        double.TryParse(parameters["Power"], out power);
                        if(power != _State.CurrentPower)
                        {
                            changed = true;
                        }
                        _State.CurrentPower = power;
                        _State.TimeStamp = DateTime.Now;
                    }

                    if(changed)
                    {
                        motor.SetMotorPower powerReq = new motor.SetMotorPower();
                        powerReq.Body.TargetPower = power;
                        _genericPort.Post(powerReq);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive<DefaultUpdateResponseType>(false, powerReq.ResponsePort,
                                    delegate(DefaultUpdateResponseType response)
                                    {
                                        HttpPostSuccess(httpPost);
                                    }),
                                Arbiter.Receive<Fault>(false, powerReq.ResponsePort,
                                    delegate(Fault fault)
                                    {
                                        HttpPostFailure(httpPost, fault.Reason[0].Value);
                                    })
                            )
                        );
                    }
                    else
                    {
                        HttpPostSuccess(httpPost);
                    }
                },
                delegate(Exception fault)
                {
                    LogError(fault.Message);
                    HttpPostFailure(httpPost, fault.Message);
                })
            );

            yield break;
        }

        /// <summary>
        /// Send Http Post Success Response
        /// </summary>
        private void HttpPostSuccess(HttpPost httpPost)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.OK, _State, _transform);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Send Http Post Failure Response
        /// </summary>
        private static void HttpPostFailure(HttpPost httpPost, string failureReason)
        {
            HttpResponseType rsp =
                new HttpResponseType(HttpStatusCode.ExpectationFailed, failureReason);
            httpPost.ResponsePort.Post(rsp);
        }

        /// <summary>
        /// Replace the state with the given update
        /// </summary>
        /// <param name="replace">Replacement state</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> ReplaceHandler(motor.Replace replace)
        {
            _State.FromGenericState(replace.Body);
            SaveState(_State);
            replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
            yield break;
        }


        /// <summary>
        /// Set the motor power level
        /// </summary>
        /// <param name="setMotorPower"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        [ServiceHandler(ServiceHandlerBehavior.Exclusive, PortFieldName = "_genericPort")]
        public IEnumerator<ITask> SetMotorPowerHandler(motor.SetMotorPower setMotorPower)
        {
            // Flip direction if nessisary
            double revPow = setMotorPower.Body.TargetPower;
            if (_State.ReversePolarity)
                revPow = (-1) * revPow;

            // Update state
            _State.CurrentPower = setMotorPower.Body.TargetPower;
            _State.TimeStamp = DateTime.Now;

            // Convert power
            int power = (int)Math.Round(revPow * 127.0);
            power += 127;
            if (power < 0)
                power = 0;
            if (power > 255)
                power = 255;

            // Send hardware specific motor data
            vex.SetMotor motordata = new vex.SetMotor();
            motordata.Motor = (vex.MotorPort)_State.MotorPort;
            motordata.Power = power;

            vex.SetMotorList motorList = new vex.SetMotorList();
            motorList.Motors = new List<vex.SetMotor>();
            motorList.Motors.Add(motordata);

            Activate(Arbiter.Choice(_vexPort.SetMotorRequest(motorList),
                delegate(DefaultUpdateResponseType status)
                {
                },
                delegate(Fault fault)
                {
                }
            ));

            // Reply to sender
            setMotorPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
    }

    /// <summary>
    /// VEX Motor Contract 
    /// </summary>
    public sealed class Contract
    {
        public const string Identifier = "http://schemas.vexrobotics.com/2011/09/vexservice.motor.html";
    }

}
